% v10: 
% v20: using Nearest Neighbor Matching and RANSAC -> coding structure
% stucks here when the spinal structure is greatly modified!!
% v30: changing the whole critical structure
% v32: neighbor region filter to enhance minDistSearch -> no need for INIT
% v40: use 4 points homography
% v50: change model data structure to a set of segments
% step
function [model_O, model_para_O] = RSpatternTrackerINIT( model,pts,r1,c1,iterNum,thDist,thInlrRatio ) 


        iterNum = 100;
        sampleNum = 1; % or 5 depending on the mode
                % user input
        % frame396
%         ptSample(1,1:4) = [117 499 508 125];
%         ptSample(2,1:4) = [447 441 71  54];
        % frame 100
%         ptSample(1,1:4) = [220 211 383 413];
%         ptSample(2,1:4) = [193 367 374  209];
        % frame 1
%         ptSample(1,1:4) = [270 214 351 418];
%         ptSample(2,1:4) = [221 355 393 273];
%         ptSample(1,1:4) = [136 108 172 208]; % small size
%         ptSample(2,1:4) = [111 177 190 137];
        
        % webcam -> don't remember how to have these numbers ?! 
%         ptSample(1,1:4) = [ 88  142 196 144]; % small size (50%) - X = (MATLAB plot X)
%         ptSample(2,1:4) = [171  207 176 132]; % Y = 0.5*imgH - (MATLAB plot Y)
        
        % pose estimation
        % frame 1 
        ptSample(1,1:4) = [108  107 194 197]; % small size (50%) - X = (MATLAB plot X)
        ptSample(2,1:4) = [122  211 214 124]; % Y = 0.5*imgH - (MATLAB plot Y)        
        % frame 2
%         ptSample(1,1:4) = [135  134 225 228]; % small size (50%) - X = (MATLAB plot X)
%         ptSample(2,1:4) = [121  215 232 125]; % Y = 0.5*imgH - (MATLAB plot Y)    
        
%         ptSample(1,1:4) = [268 211 351 420];
%         ptSample(2,1:4) = [260 122 84 206];
%         sampleNum = 1;


    ptNum = size(pts,2);
    thInlr = round(thInlrRatio*ptNum);
    inlrNum = zeros(1,iterNum);
    Hrec = zeros(3,3,iterNum);
    dist1=zeros(1,iterNum);
    movThres = 50; %100; % 'small move' threshold
    PassThres = 100;
    cntPthres = 0;
    
    % debug var
    cntsmallmov = 0;
    cntprop = 0;

% for p = 1:iterNum 
 p = 1;
    % Get current pose
    v = model(:,:,1); v1 = model(:,:,2); v2 = model(:,:,3); %v3 = model{2}(:,:,2);
    A = v(:,1);
    B = v(:,2);
    C = v1(:,2);
    D = v2(:,1);
%     E = v3(size(v3,1),:);
    pose = [A B C D];
%     pose = ptSample;
    
    % Choose samples
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % 1. fit using 1 random point, 4 others given by user input
%     sampleIdx = randIndex(size(r1,1),sampleNum);
    
%     ptSample(1,5) = c1(sampleIdx,1);
%     ptSample(2,5) = r1(sampleIdx,1);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    % Check if this is 'proper' sample set
%     if ~checkSample(ptSample),
%         cntprop = cntprop + 1;
%         continue;
%     end;
    
    % Calculate homography model H
    H = HomoMtrxCalc(pose, ptSample);
 
    % Reproject model with H
    modeli = homography(model, H);
    modeli_para = XtrackMDLpara(modeli);
    % BUG aware!! is there any case having no projection?
%     if (iscell(modeli)==0),  
%         error('Wrong input! NO homography solution!');
%         % continue; 
%     end;
    
    % 2. count the inliers, if more than thInlr, refit; else iterate 
    
    for i=1:ptNum 
            
            val = minDistSearch(pts(:,i),modeli,modeli_para);
            dist1(i) = val;
    end 
    inlier1 = find(abs(dist1) < thDist); 
    inlrNum(p) = length(inlier1) 
%     if length(inlier1) < thInlr, continue;  end 
%     if strcmp(mode,'next'),  input('run through already');end
    Hrec(:,:,p)=H;    
    ptSampleR(:,:,p)=ptSample;
%     cntPthres = cntPthres + 1; 
%     if cntPthres >= PassThres,
%         break;
%     end
% end 

% 3. choose the coef with the MOST INLINERS (not the minimum distance)
[q,idx] = max(inlrNum)% 
ret=Hrec(:,:,idx) 

modeli = homography(model, ret);
modeli_para = XtrackMDLpara(modeli);

% plotmodel(modeli);

  % model = RSpatternTracker(pts,iterNum,thDist,thInlrRatio);
    
    % Output estimation result
    % plotmodel(model);
cntsmallmov
cntprop
runcase = iterNum - cntprop - cntsmallmov
pc_mov = cntsmallmov/iterNum*100
pc_cntprop = cntprop/iterNum*100

model_O = modeli;
model_para_O = modeli_para;

end


